#include "remote_task.h"

#include "cmsis_os.h"       //FreeRTOS
#include "CAN_receive.h"    //CAN
#include "usart.h"          //usart

#include "crc.h"            //CRC

#include <string.h>

remote_data_t remote_data;

void remote_task_init(void);            //初始化
void remote_task_feedback(void);        //数据反馈
void remote_task_data_packed(void);     //数据打包
void remote_task_send(void);            //数据发送

void remote_task(void const *pvParameters) //自定义遥控器任务
{
    remote_task_init();

    while (1)
    {
        remote_task_feedback();
        remote_task_send(); 
        
        vTaskDelay(1);
    }
}

void remote_task_init(void)     //初始化
{
		memset(remote_data.data,0 ,sizeof(remote_data.data));

    //电机原始数据绑定
    remote_data.motor[0].measure = get_chassis_motor_measure_point(0);
    remote_data.motor[1].measure = get_chassis_motor_measure_point(1);
    remote_data.motor[2].measure = get_chassis_motor_measure_point(2);
    remote_data.motor[3].measure = get_chassis_motor_measure_point(3);
    remote_data.motor[4].measure = get_chassis_motor_measure_point(4);

    //电机起始位置
    remote_data.motor[0].offset_ecd = 0;
    remote_data.motor[1].offset_ecd = 0;
    remote_data.motor[2].offset_ecd = 0;
    remote_data.motor[3].offset_ecd = 0;
    remote_data.motor[4].offset_ecd = 0;

    //电机最大位置
    remote_data.motor[0].max_ecd = 0;
    remote_data.motor[1].max_ecd = 0;
    remote_data.motor[2].max_ecd = 0;
    remote_data.motor[3].max_ecd = 0;
    remote_data.motor[4].max_ecd = 0;

    //判断是否在遥控器起始位置
    
    //不在起始位置则不工作 同时发出警报 直到在起始位置上
}

void remote_task_feedback(void)     //数据反馈
{
    //关节数据处理
    remote_data.joint[0].angle_fp32 = remote_data.motor[0].measure->ecd * ECD_TO_ANGLE;
    remote_data.joint[1].angle_fp32 = remote_data.motor[1].measure->ecd * ECD_TO_ANGLE;
    remote_data.joint[2].angle_fp32 = remote_data.motor[2].measure->ecd * ECD_TO_ANGLE;
    remote_data.joint[3].angle_fp32 = remote_data.motor[3].measure->ecd * ECD_TO_ANGLE;
    remote_data.joint[4].angle_fp32 = remote_data.motor[4].measure->ecd * ECD_TO_ANGLE;

    //fp32的数据 转化为 uint8 (union避免了)

    //数据段合并赋值
    memcpy(&remote_data.data[0], remote_data.joint[0].angle_uint8, sizeof(fp32));
    memcpy(&remote_data.data[4], remote_data.joint[1].angle_uint8, sizeof(fp32));
    memcpy(&remote_data.data[8], remote_data.joint[2].angle_uint8, sizeof(fp32));
    memcpy(&remote_data.data[12], remote_data.joint[3].angle_uint8, sizeof(fp32));
    memcpy(&remote_data.data[16], remote_data.joint[4].angle_uint8, sizeof(fp32));
}

void remote_task_data_packed(void)  //数据打包
{
    static uint8_t seq = 0;
    /// 帧头数据
    remote_data.tx_data.frame_header.sof = 0xA5;                              // SOF 0xA5
    remote_data.tx_data.frame_header.data_length = 30;                // 数据帧中数据段的长度
    
    remote_data.tx_data.frame_header.seq = seq++;                             // 包序号
    append_CRC8_check_sum((uint8_t *)(&remote_data.tx_data.frame_header), 5); // CRC8
    /// 命令码ID
    remote_data.tx_data.cmd_id = CONTROLLER_CMD_ID;                           // 0x0302
    /// 数据段
    memcpy(remote_data.tx_data.data, remote_data.data, 30);                       // 160位的有效数据 
    /// 帧尾CRC16，整包校验
    append_CRC16_check_sum((uint8_t *)(&remote_data.tx_data), DATA_FRAME_LENGTH);   //CRC16
}

void remote_task_send(void)     //数据发送
{
    remote_task_data_packed();  //数据打包

    HAL_UART_Transmit(&huart6, (uint8_t *)(&remote_data.tx_data), sizeof(remote_data.tx_data), 50);
}
